#!/usr/bin/env python

import rospy
from std_msgs.msg import Int32 
from geometry_msgs.msg import PoseStamped,Twist

class ROS_INIT():
    def ros_init(self):
        rospy.init_node('target', anonymous=True)
        self.rate = rospy.Rate(20)   # Hz

        # 发布目标位置坐标
        self.pub_ugv1_cmd = rospy.Publisher('target_pos', PoseStamped, queue_size=10)



class Formation(ROS_INIT):
    def __init__(self):

        self.ros_init()
        # rospy.Timer(rospy.Duration(1/10), self.state_timer_callback, oneshot=False)      # 状态机更新state用

    def main(self):
        start_time = rospy.get_time()
        if False:
            print("ERROR!!!")
        else:
            while not rospy.is_shutdown():
                t = rospy.get_time() - start_time
                # print(t)
                x_t = 1*t
                y_t = -0.5*t
                tar_pos = PoseStamped()
                tar_pos.pose.position.x = x_t
                tar_pos.pose.position.y = y_t
                self.pub_ugv1_cmd.publish(tar_pos)
                self.rate.sleep()

    

if __name__ == "__main__":
    formation = Formation()
    formation.main()
